# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt

NUM_POINTS = 500
NUM_TIME = 40

time = np.linspace(0, NUM_TIME, NUM_POINTS) #0-40均匀取500点
realpos = np.sin(time/5) # 真实位置值
def takeMeasurement(realpos):
    '''
    function:
    ---------
    模拟生成一个测量值。生成方法：给真实位置值加高斯噪声。
    为何加高斯噪声？因为噪声一般是服从高斯分布的
    parameter:
    ----------
    @realpos: float, 某个时刻的真实位置.
    
    returns:
    --------
    @z: float, 某个时刻的位置测量值. 
    '''
    measurementNoise = .15
    z = np.random.normal(realpos, measurementNoise)
    return z

def takeOdometry(realpos_prev, realpos_curr):
    '''
    function:
    ---------
    生成一个上个时刻到当前时刻走过的距离估计值（即速度，单位时间内走过的距离不就是速度么？）。
    （在这里为了简化问题我们就不加入速度，就直接随机生成一个速度）
       
    parameters:
    -----------
    @realpos_prev: float, 前一个时刻位置真实值.
    @realpos_curr: float, 当前时刻位置真实值.
    
    returns:
    --------
    @u: float, 上个时刻到当前时刻走过的距离估计值
    '''
    processNoise = .05
    if realpos_prev == realpos_curr:
        u = 0
    else:
        u = np.random.normal(realpos_curr - realpos_prev, processNoise)
    return u
processNoise = 0.1;# 估计误差，初始化
measurementNoise = 0.1;# 测量误差，初始化

estimated_position = [] # list of best-guess estimates

x = realpos[0] # 真实位置x，初始化
p = processNoise # 方差，根据上个时刻位置估计出的当前位置的方差

unfilterOdometryReading = realpos[0] # raw odometer readings
unfilterOdometry = [] # list of odometry estimates 
unfilterMeasurements = [] # list of measurements
measurement_time = []

for i in range(len(time)):
    
    # step 1: 生成上个时刻到当前时刻这段时间走过的估计距离
    if i == 0:
        u = takeOdometry(realpos[i], realpos[i])
    else:
        u = takeOdometry(realpos[i - 1], realpos[i])
    x += u
    
    # 记录纯靠估计值所估计出的位置，用作后面的对比
    unfilterOdometryReading += u
    unfilterOdometry.append(unfilterOdometryReading)
    
    p += processNoise
    
    # 每10步进行一次测量
    if i % 10 == 0:
        z = takeMeasurement(realpos[i])#模拟一次测量
        
        # 存档测量值，用作后面的对比
        measurement_time.append(time[i]) 
        unfilterMeasurements.append(z)
        
        # 根据测量值修正位置估计值
        y = z - x
        k = p / (p + measurementNoise)
        x = x + k * y
        p = (1 - k)*p
    
    # 记录修正后的位置估计值
    estimated_position.append(x)
# 对比三种情况下的位置，和真实位置之间的差异
plt.plot(time, realpos, color = 'b') # groundtruth
plt.scatter(time, estimated_position, color = 'k', marker = 'x') # best guess
plt.scatter(measurement_time, unfilterMeasurements, color = 'r') # measurements
plt.scatter(time, unfilterOdometry, color = 'g') # odometer
plt.title('comparison of sensor readings')
plt.ylim(-2, 2)
plt.xlim(0, 40)

plt.show()
